#include "flamingdeath.h"
#include "formatted_io.h"
#include "kalman.h"
//#include "navigation.h"
#include "gps_parse.h"
#include "delay.h"
#include "compass.hpp"
#include <math.h>

extern "C" int last_value;

struct waypoint {
  float x;
  float y;
};

class flamingdeath {
private:
  /* accelerometer data */
  float accel_data[3];
  float pitch;
  float roll;
  float pitch_off;
  float roll_off;
  //float x, y, z;

  /* gps data */
  long latitude, longitude;
  uint8_t fix;
  uint8_t hdop;
  uint8_t nsatellites;

  /* read id */
  uint32_t last_accel_read_id;
  uint32_t last_gps_read_id;

  /* calibration values for accelerometer */
  float x_mul;
  float x_off;
  float y_mul;
  float y_off;
  float z_mul;
  float z_off;

  /* calibration values for compass */
  float dir_off;

  /* steering offset */
  float steering_off;
  float steeringP;
  float steeringI;
  float steeringD;
  
  /* list of waypoints */
  waypoint waypoints[40];
  uint8_t n_waypoints;

  /* formatted_io block for Bluetooth */
  formatted_io BT;

public:
  flamingdeath() :
    pitch(0.0f),
    roll(0.0f),
    pitch_off(-5.0f),
    roll_off(6.0f),
    last_accel_read_id(4134434234U),
    last_gps_read_id(4025328523U),
    x_mul(0.75/1000.0),
    x_off(-0.14f),
    y_mul(0.75/1000.0),
    y_off(-0.06f),
    z_mul(0.75/1000.0),
    z_off(0.06f),
    dir_off(33.0f),
    steering_off(0.0f),
    steeringP(5.0f/M_PI),
    steeringI(0.0f),
    steeringD(0.0f),
    n_waypoints(0),
    BT(flamingdeath_BTSend, flamingdeath_BTRecv, flamingdeath_BTRecvReady)
  {
    accel_data[0] = 0.0f;
    accel_data[1] = 0.0f;
    accel_data[2] = 0.0f;
    // initialize gps parser
    gps_init(&gps);
  }

  /**
   * Reads from the accelerometer if new data is available.  It applies calibration
   * and places the results in x, y, and z.  It returns true if new accelerometer
   * data is available, false otherwise.
   */
  bool read_accel()
  {
    if(last_accel_read_id != accel_read_id) {
      uint32_t x_raw, y_raw, z_raw;
      // spin until we read all values without the read id changing
      while(last_accel_read_id != accel_read_id) {
        last_accel_read_id = accel_read_id;
        x_raw = accel_x_raw;
        y_raw = accel_y_raw;
        z_raw = accel_z_raw;
      }

      // apply calibration
      accel_data[0] = (float)accel_x_raw * x_mul + x_off;
      accel_data[1] = (float)accel_y_raw * y_mul + y_off;
      accel_data[2] = (float)accel_z_raw * z_mul + z_off;

      pitch = atan2f(accel_data[0], accel_data[2]) + pitch_off*(float)(M_PI/180);
      roll = atan2f(accel_data[1], accel_data[2]) + roll_off*(float)(M_PI/180);

      return true;
    }
    return false;
  }


  /**
   * Reads from the gps if new data is available.  It returns true if new gps data
   * is available, false otherwise
   */
  bool read_gps()
  {
    if(last_gps_read_id != gps.id) {
      while(last_gps_read_id != gps.id) {
        last_gps_read_id = gps.id;
        latitude = gps.latitude;
        longitude = gps.longitude;
        fix = gps.fix;
        hdop = gps.hdop;
        nsatellites = gps.nsatellites;
      }
      return fix;
    }
    return false;
  }

  /**
   * Reads the compass direction and applies calibration.  Return values is the compass
   * direction in radians after calibration
   */
  float read_compass()
  {
    //FIXME: add real compass read here
    return 0.0f;//((float)compass_direction_raw / 10.0f + dir_off) * M_PI/180.0f;
  }

  void run()
  {
    BT.print("Flaming Death version 0.0.0.0.1\r\n");
	  
	  
	  // baseline gps as lat/long
	  long gpsLast[2] = {0,0};
	  
	  // distance
	  float distancexy[2] = {0.0f, 0.0f};
	  
	  // compass
	  float direction = 0.0f;
	  compass_init();
	  
	  int cnt = 0;
	  
	  //integral controller
	  float integral = 0.0f;

	  //derivative controller
	  float prevDiffHeading = 0.0f;
	  bool first = true;
	  
	  // wait for lock
	  while (1) {
		  if (read_gps()) {
			  if (nsatellites > 5) {
				  break;
			  }
			  if(++cnt == 10) {
				  // handle gps read
				  
				  float lat = latitude / 100000.0f;
				  float lon = longitude / 100000.0f;
				  
				  lat /= 60.0;
				  lon /= 60.0;
				  
				  BT.print(lat, 10);
				  BT.print(", ");
				  BT.print(lon, 10);
				  BT.print(", ");
				  BT.print((int)fix);
				  BT.print(", ");
				  BT.print((int)nsatellites);
				  BT.print(", ");
				  BT.print((int)hdop);
				  BT.print("\r\n");
				  
				  cnt = 0;
			  }
			  
		  }
		  
	  }
	  
	  while(true) {
		  
		  BT.print("Enter coordinate as x then y\r\n");
		  
		  /// TODO: make this read float
		  distancexy[0] = (float)BT.read_int();
		  distancexy[1] = (float)BT.read_int();

		  read_gps();
		  gpsLast[0] = longitude;
		  gpsLast[1] = latitude;
		  
		  uint32_t currThrottle = 3;
		  
		  throttle(currThrottle);
		  
		  
		  int16_t mag[4];
		  compass c = compass();
		  
		  cnt = 0;
		  while (fabsf(distancexy[0]) >= 5 || fabsf(distancexy[1]) >= 5) {
			  if(read_gps() && read_accel()) {
				  
				  compass_read(mag);
				  c.a.x = accel_data[0];
				  c.a.y = accel_data[1];
				  c.a.z = accel_data[2];
				  
				  c.m.x = (float)mag[0];
				  c.m.y = (float)mag[1];
				  c.m.z = (float)mag[2];
				  
				  //direction = c.heading() * M_PI/180.;
				  direction = c.heading_pitch_roll(pitch, roll, BT) * M_PI/180.;
				  
				  distancexy[0] -= diff_lon_meters(longitude,gpsLast[0]);
				  distancexy[1] -= diff_lat_meters(latitude,gpsLast[1]);
				  
				  gpsLast[0] = longitude;
				  gpsLast[1] = latitude;
				  
				  float expectedHeading = atan2f(distancexy[0],distancexy[1]);
				  float diffHeading = expectedHeading - direction;
				  
				  if (diffHeading < -M_PI) {
					  diffHeading += 2*M_PI;
				  }
				  else if (diffHeading > M_PI) {
					  diffHeading -= 2*M_PI;
				  }
				 
				  //Integral Control
				  integral += diffHeading;
				  
				  //Derivative Control
				  if (first)  {
				  	prevDiffHeading = diffHeading;
					first = false;
				  }
				  
				  float derivative = diffHeading - prevDiffHeading;
				  float sum = steeringP*diffHeading + 
				              steeringI*integral + steeringD*derivative + steering_off; 
				  steering(sum); 
				  
				  ++cnt;
				  prevDiffHeading = diffHeading;
				  if (cnt == 10) {
					  cnt = 0;
					  BT.print(distancexy[0],3);
					  BT.print("\t");
					  BT.print(distancexy[1],3);
					  BT.print("\t");
					  //BT.print((int)latitude,10);
					  //BT.print("\t");
					  //BT.print((int)longitude,10);
					  //BT.print("\t");
					  //BT.print((int)nsatellites);
					  //BT.print("\t");
					  //BT.print((int)hdop);
					  //BT.print("\r\n");
					  BT.print(expectedHeading);
					  BT.print("\t");
					  BT.print(direction);
					  BT.print("\t");
					  BT.print(sum);
					  
					  BT.print("\r\n");
				  }
			  }
			  else if (!fix) {
				  throttle(0);
				  BT.print("Lost lock\r\n");
				  break;
			  }

		  }
		  
		  throttle(0);
	  }
	  

  }

  void steering(float direction)
  {
    if(direction < -1.0f) {
    	direction = -1.0f;
    }
    else if(direction > 1.0f) {
    	direction = 1.0f;
    }
    
    int16_t dir = direction*15000.0f;
    flamingdeath_Steering(dir);
    
  }

  void throttle(unsigned speed)
  {
    switch(speed) {
      case 0: flamingdeath_Throttle(-1000); break;
      case 1: flamingdeath_Throttle(2000); break;
      case 2: flamingdeath_Throttle(3000); break;
      case 3: flamingdeath_Throttle(4000); break;
      case 4: flamingdeath_Throttle(5000); break;
      case 5: flamingdeath_Throttle(6000); break;
      case 6: flamingdeath_Throttle(7000); break;
      case 7: flamingdeath_Throttle(8000); break;
      case 8: flamingdeath_Throttle(9000); break;
      case 9: flamingdeath_Throttle(10000); break;
      case 10: flamingdeath_Throttle(12000); break;
      case 11: flamingdeath_Throttle(15000); break;
      default: flamingdeath_Throttle(-1000); break;
    }
  }

  void servo_debug()
  {
    while(1) {
      int val = BT.read_int();
      throttle(val);
      //flamingdeath_Throttle(-10000);
      //delay_ms(1000);
      //flamingdeath_Throttle(1000);
      //delay_ms(1000);
      //flamingdeath_Steering(16000);
      //delay_ms(1000);
      //flamingdeath_Steering(-16000);
      //delay_ms(1000);
    }
  }

 void gps_debug() {
	  BT.print("Read start\r\n");
    int cnt = 0;
	 while (1) {
		 
		 if(read_gps()) {
       if(++cnt == 10) {
         // handle gps read
         float direction = read_compass();

         float lat = latitude / 100000.0f;
         float lon = longitude / 100000.0f;

         lat /= 60.0;
         lon /= 60.0;
         
         BT.print(direction,6);
         BT.print(", ");
         BT.print(lat, 10);
         BT.print(", ");
         BT.print(lon, 10);
         BT.print(", ");
         BT.print((int)fix);
         BT.print(", ");
         BT.print((int)nsatellites);
         BT.print(", ");
         BT.print((int)hdop);
         BT.print("\r\n");

         cnt = 0;
       }
		 }
	 }
 }

 void gps_raw() {
   while(true) {
    char c = BT.read_char();
    flamingdeath_GPSSend(c);
   }
 }

 void simple() {
   while(true) {
     //BT.print("hello\r\n");
   }
 }

 void compass_debug() {
    BT.print("Compass Debug\r\n");
    BT.read_char();
    compass_init();

	 int16_t mag[4];
	 compass c = compass();
	 
    while(true) {
     // BT.read_char();
      delay_ms(100);
		
		compass_read(mag);
		if (read_accel()) {
			c.a.x = (float)accel_x_raw * .75 / 1000.f;
			c.a.y = (float)accel_y_raw * .75 / 1000.f;
			c.a.z = (float)accel_z_raw * .75 / 1000.f;
			
			c.m.x = (float)mag[0];
			c.m.y = (float)mag[1];
			c.m.z = (float)mag[2];
			
			float heading = c.heading();// * M_PI/180.;
			//float heading = c.heading_pitch_roll(pitch, roll, BT) * M_PI/180.;
			
			BT.print(heading);
			BT.print("\r\n");
		}
		
      //BT.print((unsigned)mx);
      //BT.print(" ");
      //BT.print((unsigned)my);
      //BT.print(" ");
      //BT.print((unsigned)mz);
      //BT.print(" ");
      //BT.print((unsigned)ms);
      //BT.print("\r\n");

    }
 }

 void accel_debug()
 {
   while(true) {
     if(read_accel()) {
       float magnitude = sqrtf(accel_data[0]*accel_data[0] + accel_data[1]*accel_data[1] + accel_data[2]*accel_data[2]);
       BT.print(accel_data[0]);
       BT.print(" ");
       BT.print(accel_data[1]);
       BT.print(" ");
       BT.print(accel_data[2]);
       BT.print(" ");
       BT.print((float)(pitch*180.0f/M_PI));
       BT.print(" ");
       BT.print((float)(roll*180.0f/M_PI));
       BT.print(" ");
       BT.print(magnitude);
       BT.print("\r\n");
     }
   }
 }
};

int main()
{
  flamingdeath_Init();

  flamingdeath fd;

  //fd.run();
	//fd.gps_debug();
  //fd.servo_debug();

  //fd.gps_raw();
  //fd.simple();
  fd.compass_debug();
  //fd.accel_debug();
}
